﻿#define B2_USE_DYNAMIC_TREE

#region Using Statements
using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;
using starLiGHT;

#endregion

#region COPYRIGHT

/*
    Copyright (c) 2008, 2009, 2010 
        Roland Rosenkranz (Glatzemann@email.de)
*/

#endregion

#region LICENSE

/*
    This file is part of starLiGHT.Collision.

    starLiGHT.Collision is free software: you can redistribute it and/or modify
    it under the terms of the GNU Lesser General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    starLiGHT.Collision is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public License
    along with starLiGHT.Collision.  If not, see <http://www.gnu.org/licenses/>.

    
    ADDITIONAL (commercial) LICENSES for starLiGHT.Collision are available on request.
*/

#endregion

#region Box2D Copyright
/*
* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/
#endregion

#region Version Stuff
// **************[ starLiGHT.Engine SVN ]**********************
// * $Rev:: 1486                  $:  Revision of last commit *
// * $Author:: glatzemann         $:  Author of last commit   *
// * $Date:: 2010-10-08 15:55:47 #$:  Date of last commit     *
// ************************************************************

// **************[ Box2D-Repository Info ]*********************
// Header-File: http://code.google.com/p/box2d/source/browse/trunk/Box2D/Box2D/Collision/b2DynamicTree.h
// Revision   : r151
// Change-Date: 2011-03-09
//
// Source-File: http://code.google.com/p/box2d/source/browse/trunk/Box2D/Box2D/Collision/b2DynamicTree.cpp
// Revision   : r151
// Change-Date: 2011-03-09
//
// Status     : Finished!
#endregion

namespace starLiGHT.Physic.TwoD
{
    public delegate float RayCastCallbackInternal(ref RayCastInput input, int userData);

#if B2_USE_DYNAMIC_TREE
    // A dynamic AABB tree broad-phase, inspired by Nathanael Presson's btDbvt.

    /// <summary>
    ///  node in the dynamic tree. The client does not interact with this directly.
    /// </summary>
    internal struct DynamicTreeNode
    {
        public bool IsLeaf()
        {
            return child1 == DynamicTree.nullNode;
        }

        // This is the fattened AABB.
        public AABB aabb;   

        public Object userData;

        public Int32 parentOrNext;

        public Int32 child1;
        public Int32 child2;
        public Int32 leafCount;
    }


    /// <summary>
    /// <para>A dynamic tree arranges data in a binary tree to accelerate 
    /// queries such as volume queries and ray casts. Leafs are proxies 
    /// with an AABB. In the tree we expand the proxy AABB by fatAABBFactor 
    /// so that the proxy AABB is bigger than the client object. This allows the client 
    /// object to move by small amounts without triggering a tree update. 
    /// </para><para>
    /// Nodes are pooled and relocatable, so we use node indices rather than pointers. 
    /// </para>
    /// </summary>
    public class DynamicTree
    {
        public static Int32 nullNode = -1;

        private Stack<int> stack = new Stack<int>(256);

        Int32 m_root;
        DynamicTreeNode[] m_nodes;
        Int32 m_nodeCount;
        Int32 m_nodeCapacity;
        
        Int32 m_freeList;

        /// This is used incrementally traverse the tree for re-balancing. 
        UInt32 m_path;

        Int32 m_insertionCount;

        /// <summary>
        /// Constructing the tree initializes the node pool.
        /// </summary>
        public DynamicTree()
        {
            m_root = nullNode;

            m_nodeCapacity = 16;
            m_nodeCount = 0;
            m_nodes = new DynamicTreeNode[m_nodeCapacity];
            
            // Build a linked list for the free list.  
            for (Int32 i = 0; i < m_nodeCapacity - 1; ++i)
            {
                m_nodes[i].parentOrNext = i + 1;
            }

            m_nodes[m_nodeCapacity-1].parentOrNext = nullNode;
            m_freeList = 0;

            m_path = 0;

            m_insertionCount = 0;
        }

        /// <summary>
        /// Destroy the tree, freeing the node pool.
        /// </summary>
        ~DynamicTree()
        {
            m_nodes = null;
        }

        /// <summary>
        /// Create a proxy. Provide a tight fitting AABB and a userData pointer. 
        /// </summary>
        public Int32 CreateProxy(AABB aabb, Object userData)
        {
            Int32 proxyId = AllocateNode();          
            
            // Fatten the aabb.         
#if !OPTIMIZE
            Vector2 r = new Vector2(PhysicSettings.aabbExtension, PhysicSettings.aabbExtension);         
#else
#if XBOX || ZUNE || WINDOWS_PHONE
            Vector2 r = Vector2.Zero;
#else
            Vector2 r; 
#endif
            r.X = PhysicSettings.aabbExtension; r.Y = PhysicSettings.aabbExtension;
#endif
            m_nodes[proxyId].aabb.lowerBound = aabb.lowerBound - r;         
            m_nodes[proxyId].aabb.upperBound = aabb.upperBound + r;         
            m_nodes[proxyId].userData = userData;
            m_nodes[proxyId].leafCount = 1;
            
            InsertLeaf(proxyId);          
            
            return proxyId;         
        }
  
        /// <summary>
        /// Destroy a proxy. This asserts if the id is invalid. 
        /// </summary>
        public void DestroyProxy(Int32 proxyId)
        {
            System.Diagnostics.Debug.Assert(0 <= proxyId && proxyId < m_nodeCapacity);
            System.Diagnostics.Debug.Assert(m_nodes[proxyId].IsLeaf());

            RemoveLeaf(proxyId);
            FreeNode(proxyId);
        }
  
        /// <summary>
        /// Move a proxy with a swepted AABB. If the proxy has moved outside of its fattened AABB, 
        /// then the proxy is removed from the tree and re-inserted. Otherwise 
        /// the function returns immediately. 
        /// </summary>
        /// <returns>true if the proxy was re-inserted.</returns>
        public bool MoveProxy(Int32 proxyId, ref AABB aabb, Vector2 displacement)
        {
            System.Diagnostics.Debug.Assert(0 <= proxyId && proxyId < m_nodeCapacity);          
            
            System.Diagnostics.Debug.Assert(m_nodes[proxyId].IsLeaf());
            
            if (m_nodes[proxyId].aabb.Contains(ref aabb))
            {
                return false;
            }
            
            RemoveLeaf(proxyId);
            
            // Extend AABB.
            AABB b = aabb;

            Vector2 r = new Vector2(PhysicSettings.aabbExtension, PhysicSettings.aabbExtension);
            b.lowerBound = b.lowerBound - r;
            b.upperBound = b.upperBound + r;

            // Predict AABB displacement.
            Vector2 d = PhysicSettings.aabbMultiplier * displacement;

            if (d.X < 0.0f)
            {
                b.lowerBound.X += d.X;
            }
            else
            {
                b.upperBound.X += d.X;
            }
            
            if (d.Y < 0.0f)
            {
                b.lowerBound.Y += d.Y;
            }
            else
            {
                b.upperBound.Y += d.Y;
            }
            
            m_nodes[proxyId].aabb = b;
            
            InsertLeaf(proxyId);

            return true; 
        }
  
        /// <summary>
        /// Perform some iterations to re-balance the tree. 
        /// </summary>
        public void Rebalance(Int32 iterations)
        {
            if (m_root == nullNode)
            {
                return;
            }

            // Rebalance the tree by removing and re-inserting leaves.
            for (Int32 i = 0; i < iterations; ++i)
            {
                int node = m_root;

                int bit = 0;
                while (m_nodes[node].IsLeaf() == false)
                {
                    Int32 children = m_nodes[node].child1;

                    //node = children + (((Int32)m_path >> bit) & 1);
                    //bit = (bit + 1) & (8 * sizeof(Int32) - 1);
                    node = ((m_path >> bit) & 1) == 0 ? m_nodes[node].child1 : m_nodes[node].child2;
                    bit = (bit + 1) & (8 * sizeof(uint) - 1);

                    //// Child selector based on a bit in the path
                    //uint selector = (m_path >> bit) & 1;

                    //// Select the child nod
                    //node = (int)(children + selector);

                    //// Keep bit between 0 and 31 because m_path has 32 bits
                    //// bit = (bit + 1) % 31;
                    //bit = (bit + 1) & 0x1F;
                }
                ++m_path;

                RemoveLeaf(node);
                InsertLeaf(node);
            }
        }

        /// <summary>
        /// Get proxy user data. 
        /// </summary>
        /// <returns>
        /// The proxy user data or NULL if the id is invalid. 
        /// </returns>
        public Object GetUserData(Int32 proxyId)
        {
            System.Diagnostics.Debug.Assert(0 <= proxyId && proxyId < m_nodeCapacity); 
            return m_nodes[proxyId].userData;
        }
  
        /// <summary>
        /// Get the fat AABB for a proxy. 
        /// </summary>
        public AABB GetFatAABB(Int32 proxyId)
        {
            System.Diagnostics.Debug.Assert(0 <= proxyId && proxyId <= m_nodeCapacity);
            return m_nodes[proxyId].aabb;
        }

        /// <summary>
        /// Compute the height of the binary tree in O(N) time. Should not be 
        /// called often.
        /// </summary>
        public Int32 ComputeHeight()
        {
            return ComputeHeight(m_root);
        }

        /// <summary>
        /// Query an AABB for overlapping proxies. The callback class 
        /// is called for each proxy that overlaps the supplied AABB. 
        /// </summary>
        public void Query(WorldQueryWrapper callback, ref AABB aabb)
        {
            stack.Clear();
            stack.Push(m_root);

            while (stack.Count > 0)
            {                 
                Int32 nodeId = stack.Pop();
                if (nodeId == nullNode)
                {
                    continue;
                }
                
                DynamicTreeNode node = m_nodes[nodeId];
                
                if (Collision.TestOverlap(ref node.aabb, ref aabb))
                {
                    if (node.IsLeaf())
                    {
                        bool proceed = callback.QueryCallback(nodeId);
                        if (proceed == false)
                        {
                            return;
                        }
                    }
                    else
                    {
                        stack.Push(node.child1);
                        stack.Push(node.child2);
                    }
                }
            } 
        }
  
        /// <summary>
        /// Ray-cast against the proxies in the tree. This relies on the callback 
        /// to perform a exact ray-cast in the case were the proxy contains a shape. 
        /// The callback also performs the any collision filtering. This has performance 
        /// roughly equal to k * log(n), where k is the number of collisions and n is the 
        /// number of proxies in the tree. 
        /// </summary>
        /// <param name="callback">a callback class that is called for each proxy that is hit by the ray.</param>
        /// <param name="input">the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).</param>
        public void RayCast(RayCastCallbackInternal callback, ref RayCastInput input)
        {
            Vector2 p1 = input.p1;
            Vector2 p2 = input.p2;
            Vector2 r = p2 - p1;
            System.Diagnostics.Debug.Assert(r.LengthSquared() > 0.0f);
            r.Normalize();

            // v is perpendicular to the segment.
            Vector2 v = Vector2Extensions.Cross(1.0f, r);
            Vector2 abs_v = v.Abs();

            // Separating axis for segment (Gino, p80).
            // |dot(v, p1 - c)| > dot(|v|, h)

            float maxFraction = input.maxFraction;

            // Build a bounding box for the segment.
            AABB segmentAABB = new AABB();
            {
                Vector2 t = p1 + maxFraction * (p2 - p1);
                segmentAABB.lowerBound = Vector2.Min(p1, t);
                segmentAABB.upperBound = Vector2.Max(p1, t);
            }

            stack.Clear();
            stack.Push(m_root);

            while (stack.Count > 0)
            {
                Int32 nodeId = stack.Pop();
                if (nodeId == nullNode)
                {
                    continue;
                }

                DynamicTreeNode node = m_nodes[nodeId];

                if (Collision.TestOverlap(ref node.aabb, ref segmentAABB) == false)
                {
                    continue;
                }

                // Separating axis for segment (Gino, p80). 
                // |dot(v, p1 - c)| > dot(|v|, h) 
                Vector2 c = node.aabb.GetCenter();
                Vector2 h = node.aabb.GetExtents();
                float separation = System.Math.Abs(Vector2.Dot(v, p1 - c)) - Vector2.Dot(abs_v, h);
                if (separation > 0.0f)
                {
                    continue;
                }

                if (node.IsLeaf())
                {
                    RayCastInput subInput;
                    subInput.p1 = input.p1;
                    subInput.p2 = input.p2;
                    subInput.maxFraction = maxFraction;

                    float value = callback(ref subInput, nodeId);

                    if (value == 0.0f)
                    {
                        // The client has terminated the ray cast.
                        return;
                    }

                    if (value > 0.0f)
                    {
                        // Update segment bounding box.
                        maxFraction = value;
                        Vector2 t = p1 + maxFraction * (p2 - p1);
                        segmentAABB.lowerBound = Vector2.Min(p1, t);
                        segmentAABB.upperBound = Vector2.Max(p1, t);
                    }
                }
                else
                {
                    stack.Push(node.child1);
                    stack.Push(node.child2);
                }
            }
        }

        public void Validate()
        {
            CountLeaves(m_root);
        }

        Int32 AllocateNode()
        {
            // Expand the node pool as needed.         
            if (m_freeList == nullNode)         
            {                 
                System.Diagnostics.Debug.Assert(m_nodeCount == m_nodeCapacity);                  
                
                // The free list is empty. Rebuild a bigger pool.                 
                DynamicTreeNode[] oldNodes = m_nodes;                 
                m_nodeCapacity *= 2;                 
                m_nodes = new DynamicTreeNode[m_nodeCapacity];                 
                Array.Copy(oldNodes, m_nodes, m_nodeCount);
                
                // Build a linked list for the free list. The parent                 
                // pointer becomes the "next" pointer.                 
                for (Int32 i = m_nodeCount; i < m_nodeCapacity - 1; ++i)                 
                {
                    m_nodes[i].parentOrNext = i + 1;                 
                }
                m_nodes[m_nodeCapacity-1].parentOrNext = nullNode;
                m_freeList = m_nodeCount;         
            }          
            
            // Peel a node off the free list.         
            Int32 nodeId = m_freeList;
            m_freeList = m_nodes[nodeId].parentOrNext;
            m_nodes[nodeId].parentOrNext = nullNode;
            m_nodes[nodeId].child1 = nullNode;
            m_nodes[nodeId].child2 = nullNode;
            m_nodes[nodeId].leafCount = 0;
            ++m_nodeCount;
            return nodeId;
        }

        void FreeNode(Int32 nodeId)
        {
            System.Diagnostics.Debug.Assert(0 <= nodeId && nodeId < m_nodeCapacity); 
            System.Diagnostics.Debug.Assert(0 < m_nodeCount); 
            m_nodes[nodeId].parentOrNext = m_freeList; 
            m_freeList = nodeId; 
            --m_nodeCount; 
        }

        void InsertLeaf(Int32 leaf)
        {
            ++m_insertionCount;
            
            if (m_root == nullNode)         
            {                 
                m_root = leaf;                 
                m_nodes[m_root].parentOrNext = nullNode;                 
                return;         
            }          
            
            // Find the best sibling for this node
            AABB leafAABB = m_nodes[leaf].aabb;

            int sibling = m_root;
            while (m_nodes[sibling].IsLeaf() == false)
            {
                int child1 = m_nodes[sibling].child1;
                int child2 = m_nodes[sibling].child2;

                // Expand the node's AABB.
                m_nodes[sibling].aabb.Combine(ref leafAABB);
                m_nodes[sibling].leafCount += 1;

                float siblingArea = m_nodes[sibling].aabb.GetPerimeter();
                AABB parentAABB = new AABB();
                parentAABB.Combine(ref m_nodes[sibling].aabb, ref leafAABB);
                float parentArea = parentAABB.GetPerimeter();
                float cost1 = 2.0f * parentArea;

                float inheritanceCost = 2.0f * (parentArea - siblingArea);

                float cost2;
                if (m_nodes[child1].IsLeaf())
                {
                    AABB aabb = new AABB();
                    aabb.Combine(ref leafAABB, ref m_nodes[child1].aabb);
                    cost2 = aabb.GetPerimeter() + inheritanceCost;
                }
                else
                {
                    AABB aabb = new AABB();
                    aabb.Combine(ref leafAABB, ref m_nodes[child1].aabb);
                    float oldArea = m_nodes[child1].aabb.GetPerimeter();
                    float newArea = aabb.GetPerimeter();
                    cost2 = (newArea - oldArea) + inheritanceCost;
                }

                float cost3;
                if (m_nodes[child2].IsLeaf())
                {
                    AABB aabb = new AABB();
                    aabb.Combine(ref leafAABB, ref m_nodes[child2].aabb);
                    cost3 = aabb.GetPerimeter() + inheritanceCost;
                }
                else
                {
                    AABB aabb = new AABB();
                    aabb.Combine(ref leafAABB, ref m_nodes[child2].aabb);
                    float oldArea = m_nodes[child2].aabb.GetPerimeter();
                    float newArea = aabb.GetPerimeter();
                    cost3 = newArea - oldArea + inheritanceCost;
                }

                // Descend according to the minimum cost.
                if (cost1 < cost2 && cost1 < cost3)
                {
                    break;
                }

                // Expand the node's AABB to account for the new leaf.
                m_nodes[sibling].aabb.Combine(ref leafAABB);

                //Descend
                if (cost2 < cost3)
                {
                    sibling = child1;
                }
                else
                {
                    sibling = child2;
                }
            }
            
            // Create a parent for the siblings.         
            int oldParent = m_nodes[sibling].parentOrNext;
            int newParent = AllocateNode();
            m_nodes[newParent].parentOrNext = oldParent;
            m_nodes[newParent].userData = null;
            m_nodes[newParent].aabb.Combine(ref leafAABB, ref m_nodes[sibling].aabb);
            m_nodes[newParent].leafCount = m_nodes[sibling].leafCount + 1;

            if (oldParent != nullNode)
            {
                // The sibling was not the root.
                if (m_nodes[oldParent].child1 == sibling)
                {
                    m_nodes[oldParent].child1 = newParent;
                }
                else
                {
                    m_nodes[oldParent].child2 = newParent;
                }

                m_nodes[newParent].child1 = sibling;
                m_nodes[newParent].child2 = leaf;
                m_nodes[sibling].parentOrNext = newParent;
                m_nodes[leaf].parentOrNext = newParent;
            }
            else
            {
                // The sibling was the root.
                m_nodes[newParent].child1 = sibling;
                m_nodes[newParent].child2 = leaf;
                m_nodes[sibling].parentOrNext = newParent;
                m_nodes[leaf].parentOrNext = newParent;
                m_root = newParent;
            } 
        }

        void RemoveLeaf(Int32 leaf)
        {
            if (leaf == m_root)
            {
                m_root = nullNode;
                return;
            }
            
            Int32 parent = m_nodes[leaf].parentOrNext;
            Int32 grandParent = m_nodes[parent].parentOrNext;
            Int32 sibling;
            if (m_nodes[parent].child1 == leaf)
            {
                sibling = m_nodes[parent].child2;
            }
            else
            {
                sibling = m_nodes[parent].child1;
            }
            
            if (grandParent != nullNode)
            {
                // Destroy parent and connect sibling to grandParent.
                if (m_nodes[grandParent].child1 == parent)
                {
                    m_nodes[grandParent].child1 = sibling;
                }
                else
                {
                    m_nodes[grandParent].child2 = sibling;
                }
                m_nodes[sibling].parentOrNext = grandParent;
                FreeNode(parent);
                
                // Adjust ancestor bounds.
                parent = grandParent;
                while (parent != nullNode)
                {
                    AABB oldAABB = m_nodes[parent].aabb;
                    m_nodes[parent].aabb.Combine(ref m_nodes[m_nodes[parent].child1].aabb, ref m_nodes[m_nodes[parent].child2].aabb);

                    System.Diagnostics.Debug.Assert(m_nodes[parent].leafCount > 0);
                    m_nodes[parent].leafCount -= 1;

                    parent = m_nodes[parent].parentOrNext;
                }
            }
            else
            {
                m_root = sibling;
                m_nodes[sibling].parentOrNext = nullNode;
                FreeNode(parent);
            } 
        }

        Int32 ComputeHeight(Int32 nodeId)
        {
            if (nodeId == nullNode)
            {
                return 0;
            }

            System.Diagnostics.Debug.Assert(0 <= nodeId && nodeId < m_nodeCapacity);
            DynamicTreeNode node = m_nodes[nodeId];
            Int32 height1 = ComputeHeight(node.child1);
            Int32 height2 = ComputeHeight(node.child2);
            return 1 + System.Math.Max(height1, height2);
        }

        Int32 CountLeaves(Int32 nodeId)
        {
            if (nodeId == nullNode)
            {
                return 0;
            }

            System.Diagnostics.Debug.Assert(0 <= nodeId && nodeId < m_nodeCapacity);
            DynamicTreeNode node = m_nodes[nodeId];

            if (node.IsLeaf())
            {
                System.Diagnostics.Debug.Assert(node.leafCount == 1);
                return 1;
            }

            int count1 = CountLeaves(node.child1);
            int count2 = CountLeaves(node.child2);
            int count = count1 + count2;
            System.Diagnostics.Debug.Assert(count == node.leafCount);
            return count;
        }
    }

#else

    public struct Proxy
    {
        // This is the fattened AABB.
        public AABB aabb;
        public object userData;
        public int id;
    }

    /// <summary>
    /// This implementation is not a tree at all. It is just a cache friendly array of AABBs.
    /// </summary>
    public class DynamicTree
    {
        public static int nullNode = -1;

        /// <summary>
        /// constructing the tree initializes the node pool.
        /// </summary>
        public DynamicTree()
        {
            m_proxyCapacity = 128;
            m_proxyCount = 0;

            m_proxyMap = new int[m_proxyCapacity];
            m_proxies = new Proxy[m_proxyCapacity];

            // Build the free list;
            m_freeId = 0;
            int last = m_proxyCapacity - 1;
            for (int i = m_freeId; i < last; ++i)
            {
                m_proxyMap[i] = i + 1;
            }

            m_proxyMap[last] = nullNode;
        }

        /// <summary>
        /// Create a proxy. Provide a tight fitting AABB and a userData pointer.
        /// </summary>
        public int CreateProxy(AABB aabb, object userData)
        {
            if (m_proxyCount == m_proxyCapacity)
            {
                m_proxyCapacity *= 2;
                int[] proxyMap = new int[m_proxyCapacity];
                Proxy[] proxies = new Proxy[m_proxyCapacity];

                Array.Copy(m_proxyMap, proxyMap, m_proxyCount);
                Array.Copy(m_proxies, proxies, m_proxyCount);

                m_proxyMap = proxyMap;
                m_proxies = proxies;

                m_freeId = m_proxyCount;
                int last = m_proxyCapacity - 1;
                for (int i = m_freeId; i < last; ++i)
                {
                    m_proxyMap[i] = i + 1;
                }

                m_proxyMap[last] = nullNode;
            }

            System.Diagnostics.Debug.Assert(0 <= m_freeId && m_freeId < m_proxyCapacity);
            int id = m_freeId;
            m_freeId = m_proxyMap[id];
            int index = m_proxyCount;

            m_proxies[index].aabb = aabb;
            m_proxies[index].userData = userData;
            m_proxies[index].id = id;
            m_proxyMap[id] = index;
            ++m_proxyCount;

            return id;
        }

        /// <summary>
        /// Destroy a proxy. This asserts if the id is invalid.
        /// </summary>
        public void DestroyProxy(int proxyId)
        {
            System.Diagnostics.Debug.Assert(0 < m_proxyCount && 0 <= proxyId && proxyId < m_proxyCapacity);
            int index = m_proxyMap[proxyId];

            // add to free list
            m_proxyMap[proxyId] = m_freeId;
            m_freeId = proxyId;

            // Keep proxy array contiguous
            if (index < m_proxyCount - 1)
            {
                m_proxies[index] = m_proxies[m_proxyCount - 1];
                int id = m_proxies[index].id;
                m_proxyMap[id] = index;
            }

            --m_proxyCount;

            Validate();
        }

        /// <summary>
        /// Move a proxy with a swepted AABB. If the proxy has moved outside of its fattened AABB,
        /// then the proxy is removed from the tree and re-inserted. Otherwise 
        /// the functions returns immediately.
        /// </summary>
        /// <returns>
        /// true if the proxy was re-inserted.
        /// </returns>
        public bool MoveProxy(int proxyId, ref AABB aabb1, Vector2 displacement)
        {
            System.Diagnostics.Debug.Assert(0 < m_proxyCount && 0 <= proxyId && proxyId < m_proxyCapacity);

            int index = m_proxyMap[proxyId];

            if (m_proxies[index].aabb.Contains(ref aabb1))
            {
                return false;
            }

            // Extend AABB.
            AABB b = aabb1;
            Vector2 r = new Vector2(PhysicSettings.aabbExtension, PhysicSettings.aabbExtension);
            b.lowerBound = b.lowerBound - r;
            b.upperBound = b.upperBound + r;

            // Predict AABB displacement.
            Vector2 d = PhysicSettings.aabbMultiplier * displacement;

            if (d.X < 0f)
            {
                b.lowerBound.X += d.X;
            }
            else
            {
                b.upperBound.X += d.X;
            }

            if (d.Y < 0f)
            {
                b.lowerBound.Y += d.Y;
            }
            else
            {
                b.upperBound.Y += d.Y;
            }

            m_proxies[index].aabb = b;

            return true;
        }

        /// <summary>
        /// Perform some iterations to re-balance the tree.
        /// </summary>
        public void Rebalance(int iterations)
        {

        }

        /// <summary>
        /// Get proxy user data.
        /// </summary>
        /// <returns>
        /// The proxy user data or null if the id is invalid.
        /// </returns>
        public object GetUserData(int proxyId)
        {
            System.Diagnostics.Debug.Assert(0 <= proxyId && proxyId < m_proxyCapacity);
            int index = m_proxyMap[proxyId];
            return m_proxies[index].userData;
        }

        /// <summary>
        /// Get the fat AABB for a proxy.
        /// </summary>
        public AABB GetFatAABB(int proxyId)
        {
            System.Diagnostics.Debug.Assert(0 <= proxyId && proxyId < m_proxyCapacity);
            int index = m_proxyMap[proxyId];
            return m_proxies[index].aabb;
        }

        /// <summary>
        /// Compute the height of the binary tree in O(N) time. Should not be called often.
        /// </summary>
        /// <returns></returns>
        public int ComputeHeight()
        {
            return 0;
        }

        /// <summary>
        /// Query an AABB for overlapping proxies. The callback class 
        /// is called for each proxy that overlaps the supplied AABB. 
        /// </summary>
        public void Query(WorldQueryWrapper callback, ref AABB aabb)
        {
            for (int i = 0; i < m_proxyCount; ++i)
            {
                if (Collision.TestOverlap(ref m_proxies[i].aabb, ref aabb))
                {
                    bool proceed = callback.QueryCallback(m_proxies[i].id);
                    if (proceed == false)
                    {
                        return;
                    }
                }
            }
        }

        /// <summary>
        /// Ray-cast against the proxies in the tree. This relies on the callback 
        /// to perform a exact ray-cast in the case were the proxy contains a shape. 
        /// The callback also performs the any collision filtering. This has performance 
        /// roughly equal to k * log(n), where k is the number of collisions and n is the 
        /// number of proxies in the tree. 
        /// </summary>
        /// <param name="callback">a callback class that is called for each proxy that is hit by the ray.</param>
        /// <param name="input">the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).</param>
        public void RayCast(RayCastCallbackInternal callback, ref RayCastInput input)
        {
            Vector2 p1 = input.p1;
            Vector2 p2 = input.p2;
            Vector2 r = p2 - p1;
            System.Diagnostics.Debug.Assert(r.LengthSquared() > 0f);
            r.Normalize();

            // v is perpendicular to the segment.
            Vector2 v = Vector2Extensions.Cross(1.0f, r);
            Vector2 abs_v = Vector2Extensions.Abs(v);

            // Separating axis for segment (Gino, p80).
            // |dot(v, p1 - c| > dot(|v|, h)

            float maxFraction = input.maxFraction;

            // Build a bounding box for the segment
            AABB segmentAABB = new AABB();
            {
                Vector2 t = p1 + maxFraction * (p2 - p1);
                segmentAABB.lowerBound = Vector2.Min(p1, t);
                segmentAABB.upperBound = Vector2.Max(p1, t);
            }

            for (int i = 0; i < m_proxyCount; ++i)
            {
                Proxy proxy = m_proxies[i];
                AABB proxyAABB = proxy.aabb;

                if (Collision.TestOverlap(ref proxyAABB, ref segmentAABB) == false)
                {
                    continue;
                }

                // Separating axis for segment (Gino, p80).
                // |dot(v, p1 - c| > dot(|v|, h)
                Vector2 c = proxyAABB.GetCenter();
                Vector2 h = proxyAABB.GetExtents();
                float separation = (float)System.Math.Abs(Vector2.Dot(v, p1 - c)) - Vector2.Dot(abs_v, h);

                if (separation > 0f)
                {
                    continue;
                }

                RayCastInput subInput = new RayCastInput();
                subInput.p1 = input.p1;
                subInput.p2 = input.p2;
                subInput.maxFraction = maxFraction;

                float value = callback(ref subInput, proxy.id);

                if (value == 0f)
                {
                    // the client has terminated the ray cast.
                    return;
                }

                if (value > 0f)
                {
                    maxFraction = value;
                    Vector2 t = p1 + maxFraction * (p2 - p1);
                    segmentAABB.lowerBound = Vector2.Min(p1, t);
                    segmentAABB.upperBound = Vector2.Max(p1, t);
                }
            }
        }

        public void Validate()
        {
            System.Diagnostics.Debug.Assert(m_proxyCount > 0 || m_freeId == nullNode);
            System.Diagnostics.Debug.Assert(m_freeId == nullNode || m_freeId < m_proxyCapacity);

            int id = m_freeId;
            int freeCount = 0;
            while (id != nullNode)
            {
                ++freeCount;
                System.Diagnostics.Debug.Assert(freeCount <= m_proxyCapacity);
                id = m_proxyMap[id];
            }

            System.Diagnostics.Debug.Assert(freeCount + m_proxyCount == m_proxyCapacity);
            System.Diagnostics.Debug.Assert(m_proxyCount <= m_proxyCapacity);

            for (int i = 0; i < m_proxyCount; ++i)
            {
                int _id = m_proxies[i].id;
                System.Diagnostics.Debug.Assert(m_proxyMap[_id] == i);
            }
        }

        private int[] m_proxyMap;
        private Proxy[] m_proxies;

        private int m_proxyCount;
        private int m_proxyCapacity;

        private int m_freeId;
    }
#endif
}
